#include "DetectFinger.h"

void* detectFinger(void* argument) {
	argumentDataSet_t* argumentsData = (argumentDataSet_t*) argument;
	int x1, x2, y1, y2;
	int x, y, range;
	int xRange, yRange;
	int i;
	int all=0;
	float circumference, deltaTheta, theta;
	coordinate_t triPoint;
	queue_t* unitQueue = NULL;

	initQueue(unitQueue);


	x1 = argumentsData->interestedArea.point1.x;
	y1 = argumentsData->interestedArea.point1.y;
	x2 = argumentsData->interestedArea.point2.x;
	y2 = argumentsData->interestedArea.point2.y;

	range = MAX((y2 - y1), (x2 - x1)) * argumentsData->rangeRate;
	//printf("range : %f\n", argumentsData->rangeRate);

	/* if not range < 0, then draw circle and enqueue */
	if (range < 0)
	{
		return NULL;
	}

	circumference = range * 2 * PI;		//number of pixel circumference
	if (circumference < 1)
	{
		circumference = 0.5;
	}
	deltaTheta = 360 / circumference;


	
	for ( int y = y1; y < y2; y++ )
	{
		for ( int x = x1; x < x2; x++ )
		{			
			int temp = (y*IMG_WIDTH + x) * 3;
			if (argumentsData->image->imageData[temp] == 255)
			{
				all++;
			}
		}
	}
	

	//printf("----------- insert data ----------\n");
	/* insert data and coordinates in queue */
	for (theta = 0; theta < 360; theta += deltaTheta)
	{

		//printf("%f %f\n", theta, deltaTheta);
		//printf("%d %d\n", argumentsData->point.x, argumentsData->point.y);
		//todo cos sin lookup table
		triPoint.x = cos(theta * PI / 180.0)*range + argumentsData->point.x;
		triPoint.y = sin(theta * PI / 180.0)*range + argumentsData->point.y;

		int temp = (triPoint.y*IMG_WIDTH + triPoint.x) * 3;

		if (temp < 0 || temp > (IMG_WIDTH * IMG_HEIGHT * 3))
		{
			temp = 0;
		}
		

		if (argumentsData->image->imageData[temp] == 255)
		{

			enqueue(unitQueue, 1, triPoint);

			if (triPoint.x >= 0 &&
				triPoint.x < IMG_WIDTH &&
				triPoint.y >= 0 &&
				triPoint.y < IMG_HEIGHT)
			{
				argumentsData->image->imageData[temp] = 0;
				argumentsData->image->imageData[temp + 1] = 0;
				argumentsData->image->imageData[temp + 2] = 255;
			}
		}
		else
		{
			enqueue(unitQueue, 0, triPoint);

			if (triPoint.x >= 0 &&
				triPoint.x < IMG_WIDTH &&
				triPoint.y >= 0 &&
				triPoint.y < IMG_HEIGHT)
			{
				argumentsData->image->imageData[temp] = 0;
				argumentsData->image->imageData[temp + 1] = 255;
				argumentsData->image->imageData[temp + 2] = 0;
			}

		}
	}

	//printf("----------- midian filter ----------\n");
	//binary midian filtering
	int margin = 3;
	int data[9];
	int count = 0;
	int m;
	queue_t* tempQueue = NULL;	//filtered Queue;
	initQueue(tempQueue);
	for (i = margin; i < unitQueue->length - margin; i++)
	{
		count = 0;
		for (m = -margin; m < margin; m++)
		{
			if (nodeAtQueue(unitQueue, i + m)->data == 1)
			{
				count++;
			}
		}

		if (count > 3)
		{
			enqueue(tempQueue, 1, nodeAtQueue(unitQueue, i)->point);
		}
		else{
			enqueue(tempQueue, 0, nodeAtQueue(unitQueue, i)->point);
		}
	}

	//printf("----------- find finger position ----------\n");
	//find finger's position and how much thick
	int queue_length = unitQueue->length;
	int start, end;
	int check = 0;

	queue_t* fingerQueue = NULL;
	start = 0;
	end = 0;
	initQueue(fingerQueue);
	for (i = 0; i < queue_length; i++)
	{
		node_t* tempNode = nodeAtQueue(tempQueue, i);			

		if (check)
		{
			
			if (tempNode != NULL && tempNode->data == 0)
			{
				check = 0;
				end = i;
				// how much thick and where is the finger				
				enqueue(fingerQueue, end - start, nodeAtQueue(tempQueue, start + (end - start) / 2)->point);
			}
		}
		else
		{
			if (tempNode != NULL && tempNode->data == 1)
			{
				check = 1;
				start = i;
			}
		}
	}


	//printf("----------- print the position ----------\n");
	//print the position
	while (fingerQueue->length > 0)
	{
		node_t* tempNode = nodeAtQueue(fingerQueue, 0);
		//printf("all : %d\tthick : %d\tx : %d\t y : %d\n", all, tempNode->data, tempNode->point.x, tempNode->point.y);

		if (triPoint.x >= 0 &&
			triPoint.x < IMG_WIDTH &&
			triPoint.y >= 0 &&
			triPoint.y < IMG_HEIGHT)
		{
		//	if (tempNode->data > 0)
				cvCircle(argumentsData->image, cvPoint(tempNode->point.x, tempNode->point.y), 3, CV_RGB(0, 0, 255), -1);
		}

		dequeue(fingerQueue);
	}

	
	//printf("----------- enD ----------\n");
	/* queue memory free */
	freeQueue(tempQueue);
	freeQueue(fingerQueue);
	freeQueue(unitQueue);

	return NULL;
}

/* set arguments for finger thread */
void setArguments(argumentDataSet_t* argumentsData,
				  IplImage* image,
				  areaCoordinate_t interestedArea,
				  coordinate point,
				  float rangeRate) {
	argumentsData->image = image;
	argumentsData->interestedArea = interestedArea;
	argumentsData->point = point;
	argumentsData->rangeRate = rangeRate;
}













